#include "bucketcheck.h"
#include "location.h"
#include <ros/ros.h>
#include <std_msgs/Int32.h>
#include <sensor_msgs/LaserScan.h>
#include <geometry_msgs/PoseArray.h>
#include <geometry_msgs/Pose.h>
#include <image_transport/image_transport.h>
#include <cv_bridge/cv_bridge.h>
#include <sensor_msgs/image_encodings.h>
#include <tf/transform_listener.h>
#include <sensor_msgs/CameraInfo.h>
#include <Eigen/Dense>
#include <Eigen/Geometry>

#include <message_filters/subscriber.h>
#include <message_filters/synchronizer.h>
#include <message_filters/sync_policies/approximate_time.h>
#include <message_filters/time_synchronizer.h>

using namespace message_filters;
using namespace sensor_msgs;
using namespace Eigen;
using namespace std;
using namespace cv;
std::string map_frame_;

int main(int argc, char** argv)
{
  ros::init(argc,argv,"bucketcheck");
  ros::NodeHandle nh_;
  ros::NodeHandle nh_private("~");
  ros::Rate rate(20);
  ros::Publisher marker_pub = nh_.advertise<visualization_msgs::Marker>("visualization_marker", 10);
  ros::Publisher pub = nh_.advertise<geometry_msgs::PoseArray>("/bucket_pose",20);

  ros::Duration seconds(0.2);
  nh_private.param("map_frame",map_frame_,std::string("map"));

  tf::TransformListener listener;

  vector<Point2f> location;
  vector<Point2f> location_select;
  Location location_bucket;
  Location location_bucket_target;
  BucketCheck bucket;

  while(ros::ok()){

    if(bucket.data_.size()>0){
      geometry_msgs::PoseArray pose_bucket;
      geometry_msgs::Pose bucket_pose;

      vector<Eigen::Vector4d> pose_array;
      vector<Eigen::Vector4d> pose_array_select;

      vector<Eigen::VectorXf> target_model_circle;  //目标尺寸条桶检测结果
      vector<Eigen::VectorXf> select_model_circle;  //干扰尺寸条桶检测结果
      bucket.bucketcheck(bucket.data_, select_model_circle,target_model_circle);

      bucket.bucketcheck(bucket.data_,select_model_circle,target_model_circle);
      tf::StampedTransform transform_laser;
      try{
        listener.lookupTransform(map_frame_,bucket.frame_id_,ros::Time(0),transform_laser);
        Eigen::Isometry3d t1 = Eigen::Isometry3d::Identity();
        double* dt1 =  t1.data();
        dt1[0]=transform_laser.getBasis()[0][0];
        dt1[1]=transform_laser.getBasis()[1][0];
        dt1[2]=transform_laser.getBasis()[2][0];
        dt1[3]=0;
        dt1[4]=transform_laser.getBasis()[0][1];
        dt1[5]=transform_laser.getBasis()[1][1];
        dt1[6]=transform_laser.getBasis()[2][1];
        dt1[7]=0;
        dt1[8]=transform_laser.getBasis()[0][2];
        dt1[9]=transform_laser.getBasis()[1][2];
        dt1[10]=transform_laser.getBasis()[2][2];
        dt1[11]=0;
        dt1[12]=transform_laser.getOrigin().x();
        dt1[13]=transform_laser.getOrigin().y();
        dt1[14]=0;
        dt1[15]=1;
        vector<Vector4d> pose_laser;
        vector<Vector4d> pose_laser_target;
        Eigen::Isometry3d t2 = t1;

      if(location_bucket_target.detection_reset_ == 1){
          location.clear();
          location_select.clear();
      }
        location_bucket.follow_location(pose_laser,select_model_circle,t1,location_select,pose_array_select,location_bucket.detection_reset_);
        location_bucket_target.follow_location(pose_laser_target,target_model_circle,t2,location,pose_array,location_bucket_target.detection_reset_);
         if(pose_array_select.size()!=0){
           for(int i=0;i<pose_array_select.size();i++){
             for(int j=0;j<pose_array.size();){
               if(sqrt((pose_array_select[i][0]-pose_array[j][0])*(pose_array_select[i][0]-pose_array[j][0])+
                  (pose_array_select[i][1]-pose_array[j][1])*(pose_array_select[i][1]-pose_array[j][1]))<bucket.select_bucket_radius_/100){
                 pose_array.erase(pose_array.begin()+j);
               }
               else{
                 ++j;
               }
             }
           }
         }
      }
      catch(tf::TransformException &ex){
        //ROS_ERROR("%s",ex.what());
        ros::Duration(0.02).sleep();
      }

      if(pose_array.size()!=0){
        for(int i=0;i<pose_array.size();i++)
        {
          bucket_pose.position.x=pose_array[i][0];
          bucket_pose.position.y=pose_array[i][1];
          bucket_pose.position.z=0;
          pose_bucket.poses.push_back(bucket_pose);
        }
        pub.publish(pose_bucket);
      }
    }
    rate.sleep();
    ros::spinOnce();
  }
  return 0;
}
